# -*- coding: utf-8 -*-
# @Time    : 2024/9/9 22:12
# @Author  : Z.J. Zhang
# @Email   : zijingzhang@mail.ustc.edu.cn
# @File    : geometrical.py
# @Software: PyCharm
import numpy


class Line:
    def __init__(self, point_in_line: numpy.ndarray, direction_vec: numpy.ndarray, ):
        self.point_in_line = point_in_line
        self.direction_vec = direction_vec


class Plane:
    def __init__(self, point_in_plane: numpy.ndarray, normal_vec_of_plane: numpy.ndarray):
        self.point_in_plane = point_in_plane
        self.normal_vec_of_plane = normal_vec_of_plane
        self.intersection_with_line = numpy.vectorize(self._intersection_with_line, signature="(3) ,(3)-> (3)")

    def intersection_with_parallel_lines(self, points_in_lines: numpy.ndarray, direction_vec: numpy.ndarray):
        """
        :param points_in_lines: shape (N_points,3)
        :param direction_vec:
        :return:
        """
        # ndim = points_in_line.ndim
        old_shape = points_in_lines.shape
        points_in_lines = points_in_lines.reshape((-1, 3))


        nx, ny, nz = self.normal_vec_of_plane
        x0, y0, z0 = self.point_in_plane
        x1, y1, z1 = points_in_lines.T
        kx, ky, kz = direction_vec
        N_points = len(points_in_lines)
        M = numpy.matrix([
                             [nx, ny, nz, ],
                             ([ky, -kx, 0]),
                             ([0, kz, -ky]),
                             ([-kz, 0, kx])
                         ]  )

        b = numpy.matrix([
            [nx * x0 + ny * y0 + nz * z0] * N_points,
            (ky * x1 - kx * y1),
            (kz * y1 - ky * z1),
            (-kz * x1 + kx * z1)

        ])
        return (numpy.linalg.pinv(M) * b).A.T.reshape(old_shape)

    def intersection_with_parallel_lines2(self, points_in_line: numpy.ndarray, direction_vec: numpy.ndarray):
        """
        :param points_in_line: shape (...,3)
        :param direction_vec:
        :return:
        """
        nx, ny, nz = self.normal_vec_of_plane
        x0, y0, z0 = self.point_in_plane
        x1, y1, z1 = points_in_line.T
        kx, ky, kz = direction_vec
        N_points = len(points_in_line)
        M = numpy.matrix([
                             [nx, ny, nz, ],
                             ([ky, -kx, 0]),
                             ([0, kz, -ky]),
                             ([-kz, 0, kx])
                         ]  )

        b = numpy.matrix([
            [nx * x0 + ny * y0 + nz * z0] * N_points,
            (ky * x1 - kx * y1),
            (kz * y1 - ky * z1),
            (-kz * x1 + kx * z1)

        ])
        return (numpy.linalg.pinv(M) * b).A.T

    def _intersection_with_line(self, point_in_line: numpy.ndarray, direction_vec_of_line: numpy.ndarray):
        nx, ny, nz = self.normal_vec_of_plane
        x0, y0, z0 = self.point_in_plane
        x1, y1, z1 = point_in_line
        kx, ky, kz = direction_vec_of_line
        M = numpy.matrix([
            [nx, ny, nz, ],
            [ky, -kx, 0],
            [0, kz, -ky],
            [-kz, 0, kx]
        ])

        b = numpy.matrix([[nx * x0 + ny * y0 + nz * z0,
                           ky * x1 - kx * y1,
                           kz * y1 - ky * z1,
                           -kz * x1 + kx * z1
                           ]]).T
        return (numpy.linalg.pinv(M) * b).A.T[0]

    def projected_pt(self, pt: numpy.ndarray):
        return self.intersection_with_line(pt, self.normal_vec_of_plane)


if __name__ == '__main__':
    plane = Plane(numpy.array([0, 0, 1]), numpy.array([0, 0, 1]))
    plane.projected_pt(numpy.array([[2, 2, 3],
                                    [2, 1, 3], ]))
